#include "sci.h"
#include "js.h"
#include "lcd.h"

void parse_joy_stick(const char *msg) {
  int i,j,k;

  char out[MAXJSSIZE]; // output buffer
  char *t, t1[MAXJSSIZE];
  
  // number conversion variables
  char char_xaxis[7], char_yaxis[7];
  int tnum, neg1, neg2, xaxis, yaxis, delNum, xaxis_size, yaxis_size;

  char header[2] = {'X', 'Y'}; // Defined header
  char delimit = ',';  // Defined delimiter
  char end = 0; // Defined end character
  
  t = t1;
  delNum = 0;
  xaxis_size = 0;
  yaxis_size = 0;
  xaxis = 0;   
  yaxis = 0;
  neg1 = 1;
  neg2 = 1;
  j = 0;
  k = 0; // display index
 
  // initialize temporary numbers for display
  for(i = 0; i < MAXJSNUM; i++){
    char_xaxis[i] = '\0';
    char_yaxis[i] = '\0';
  }

  // initialize output buffer for display
  for(i = 0; i<MAXJSSIZE; i++) {
    
    out[i] = ' ';
    t1[i] = '*';
  }

    i = 0;
  // Look for header
    while(msg[i] != header[0] && msg[i+1] != header[1])
        i++;
    i+=2;

    // Parse Joystick Data
    
    while(msg[++i] != end)
    {
        t1[i] = msg[i];  // test read in phrase without identifiers
        if(msg[i] != delimit)
        {
            if(delNum == 0) // First Number
            {
                out[k++] = msg[i]; // for display
                char_xaxis[j++] = msg[i];
                xaxis_size++;
            }
            if(delNum == 1) // Second Number
            {
                out[k++] = msg[i]; // for display
                char_yaxis[j++] = msg[i];
                yaxis_size++;
            }
            if(delNum == 2); // for third variable, button, or whatever

        }
        else
        {
            out[k++] = ' '; // for display
            delNum++;
            j = 0;
        }
    }

    

    // Convert to Decimal Numbers
    // X axis
    tnum = 1;
    for(j = 0; j<xaxis_size; j++) // reverse order
    {
        if(char_xaxis[xaxis_size-j-1] == '-')
        {
            neg1 = -1;
            xaxis_size--;    // a '-' adds an extra element to xaxis_size, should be at the end, so shouldn't screw up indexing
        }
        else
        {
            if(j != 0)
              tnum *= 10;
            xaxis += (int)(char_xaxis[xaxis_size-j-1] - 0x30) * tnum;
        }
    }
    // Y axis
    tnum = 1;
    for(j = 0; j<yaxis_size; j++) // reverse order
    {
        if(char_yaxis[yaxis_size-j-1] == '-')
        {
            neg2 = -1;
            yaxis_size--;    // a '-' adds an extra element to yaxis_size, should be at the end, so shouldn't screw up indexing
        }
        else
        {
            if(j != 0)
              tnum *= 10;
            yaxis += (int)(char_yaxis[yaxis_size-j-1] - 0x30) * tnum;
        }
    }

    xaxis *=neg1;
    yaxis *=neg2;
    
       
  writeLine(out,1);  // Outputs to LCD Second Line 
  writeLine(t,0);  // Displays test phrase
  
}